#ifndef _DALIN_CONTROLLER_H_
#define _DALIN_CONTROLLER_H_

#ifndef __API__
#define __API__
#endif

#ifndef __STATIC_INLINE__  
#define __STATIC_INLINE__ static inline
#endif

struct DahlinController {
    float last_error;
    float u;
    float last_u;
    float previous_u;
    float a0;
    float a1;
    float b0;
    float b1;
};

__API__ __STATIC_INLINE__ int DahlinController_Init(struct DahlinController *dc)
{
    if (!dc) return -1;

	dc->last_error   = 0;
    dc->u            = 0;
    dc->last_u       = 0;
    dc->previous_u   = 0;
    dc->a0           = 0;
    dc->a1           = 0;
    dc->b0           = 0;
    dc->b1           = 0;
    return 0;
}


__API__ __STATIC_INLINE__ int DahlinController_a0_Set(struct DahlinController *dc, float a0)
{
    if (!dc) return -1;

    dc->a0 = a0;
    return 0;
}


__API__ __STATIC_INLINE__ int DahlinController_a1_Set(struct DahlinController *dc, float a1)
{
    if (!dc) return -1;

    dc->a1 = a1;
    return 0;
}


__API__ __STATIC_INLINE__ int DahlinController_b0_Set(struct DahlinController *dc, float b0)
{
    if (!dc) return -1;

    dc->b0 = b0;
    return 0;
}


__API__ __STATIC_INLINE__ int DahlinController_b1_Set(struct DahlinController *dc, float b1)
{
    if (!dc) return -1;

    dc->b1 = b1;
    return 0;
}


__API__ __STATIC_INLINE__ float DahlinControl(struct DahlinController *dc, float target, float actual)
{
    float error;

    error = target - actual;

	dc->u = dc->b0*dc->last_u + dc->b1*dc->previous_u + dc->a0*error + dc->a1*dc->last_error;  /* Dahlin equation */

    dc->last_error = error;
	dc->previous_u = dc->last_u;
	dc->last_u = dc->u;

	return dc->u;
}


#endif /* _DALIN_CONTROLLER_H_ */